/*********************************************************************\
*
*                  (c) lkc
*                  All Rights Reserved. lkc Confidential
*
\*********************************************************************/
/*********************************************************************\
*   Title                      :
*
*   Module Description         :
*
*   Author                     : lkc
*
*   Created                    : 2018-8-15
*
\*********************************************************************/

#include "FocusControl.h"
#include "ProtocolMCU.h"
#include <math.h>

static const int FOCUS_MAX = 150;
static const int FOCUS_MIN = 50;
static const float ICCD_CATHODE_RADIUS = 9.0;
static const double RADIAN = 57.29577951;

float CheckFocusRange(float focus)//判断焦距是否超过最大最小值
{
    if(focus>FOCUS_MAX)
    {
        return FOCUS_MAX;
    }
    if(focus<FOCUS_MIN)
    {
        return FOCUS_MIN;
    }
    return focus;
}

#ifndef MCU_IS_ALIVE
#include "MotorDrv8885.h"
#include "Ad7680.h"
#include<algorithm>
#include <functional>
#include <thread>
#include "CamControl.h"

static float ratioTable[] = { 0.05049 ,0.05244 ,0.05438 ,0.05631 ,0.05822 ,0.06013 ,0.06202 ,0.06391 ,0.06578 ,0.06764 ,0.06949 ,0.07133 ,0.07316 ,0.07498 ,0.07679 ,0.07859 ,0.08038 ,0.08215 ,0.08392 ,0.08568 ,0.08743 ,0.08916 ,0.09089 ,0.09261 ,0.09432 ,0.09602 ,0.09771 ,0.09939 ,0.10106 ,0.10272 ,0.10437 ,0.10602 ,0.10765 ,0.10927 ,0.11089 ,0.11249 ,0.11409 ,0.11568 ,0.11726 ,0.11883 ,0.12039 ,0.12195 ,0.12349 ,0.12503 ,0.12656 ,0.12808 ,0.12959 ,0.13109 ,0.13259 ,0.13407 ,0.13555 ,0.13702 ,0.13848 ,0.13994 ,0.14138 ,0.14282 ,0.14425 ,0.14568 ,0.14709 ,0.14850 ,0.14990 ,0.15130 ,0.15268 ,0.15406 ,0.15543 ,0.15679 ,0.15815 ,0.15950 ,0.16084 ,0.16218 ,0.16350 ,0.16482 ,0.16614 ,0.16745 ,0.16875 ,0.17004 ,0.17133 ,0.17261 ,0.17388 ,0.17515 ,0.17641 ,0.17766 ,0.17891 ,0.18015 ,0.18138 ,0.18261 ,0.18383 ,0.18505 ,0.18626 ,0.18746 ,0.18866 ,0.18985 ,0.19103 ,0.19221 ,0.19339 ,0.19455 ,0.19571 ,0.19687 ,0.19802 ,0.19916 ,0.20030 ,0.20144 ,0.20256 ,0.20368 ,0.20480 ,0.20591 ,0.20702 ,0.20812 ,0.20921 ,0.21030 ,0.21138 ,0.21246 ,0.21354 ,0.21461 ,0.21567 ,0.21673 ,0.21778 ,0.21883 ,0.21987 ,0.22091 ,0.22194 ,0.22297 ,0.22400 ,0.22501 ,0.22603 ,0.22704 ,0.22804 ,0.22904 ,0.23004 ,0.23103 ,0.23202 ,0.23300 ,0.23397 ,0.23495 ,0.23592 ,0.23688 ,0.23784 ,0.23879 ,0.23975 ,0.24069 ,0.24164 ,0.24257 ,0.24351 ,0.24444 ,0.24536 ,0.24629 ,0.24720 ,0.24812 ,0.24903 ,0.24993 ,0.25084 ,0.25173 ,0.25263 ,0.25352 ,0.25440 ,0.25529 ,0.25617 ,0.25704 ,0.25791 ,0.25878 ,0.25965 ,0.26051 ,0.26136 ,0.26222 ,0.26307 ,0.26391 ,0.26476 ,0.26560 ,0.26643 ,0.26726 ,0.26809 ,0.26892 ,0.26974 ,0.27056 ,0.27138 ,0.27219 ,0.27300 ,0.27380 ,0.27461 ,0.27541 ,0.27620 ,0.27700 ,0.27779 ,0.27857 ,0.27936 ,0.28014 ,0.28092 ,0.28169 ,0.28247 ,0.28323 ,0.28400 ,0.28476 ,0.28552 ,0.28628 ,0.28704 ,0.28779 ,0.28854 ,0.28928 ,0.29003 ,0.29077 ,0.29151 ,0.29224 ,0.29297 ,0.29370 ,0.29443 ,0.29516 ,0.29588 ,0.29660 ,0.29731 ,0.29803 ,0.29874 ,0.29945 ,0.30016 ,0.30086 ,0.30156 ,0.30226 ,0.30296 ,0.30365 ,0.30435 ,0.30503 ,0.30572 ,0.30641 ,0.30709 ,0.30777 ,0.30845 ,0.30912 ,0.30980 ,0.31047 ,0.31114 ,0.31180 ,0.31247 ,0.31313 ,0.31379 ,0.31445 ,0.31510 ,0.31576 ,0.31641 ,0.31706 ,0.31770 ,0.31835 ,0.31899 ,0.31963 ,0.32027 ,0.32091 ,0.32154 ,0.32218 ,0.32281 ,0.32344 ,0.32406 ,0.32469 ,0.32531 ,0.32593 ,0.32655 ,0.32717 ,0.32778 ,0.32840 ,0.32901 ,0.32962 ,0.33022 ,0.33083 ,0.33143 ,0.33204 ,0.33264 ,0.33324 ,0.33383 ,0.33443 ,0.33502 ,0.33561 ,0.33620 ,0.33679 ,0.33738 ,0.33796 ,0.33854 ,0.33913 ,0.33970 ,0.34028 ,0.34086 ,0.34143 ,0.34201 ,0.34258 ,0.34315 ,0.34371 ,0.34428 ,0.34485 ,0.34541 ,0.34597 ,0.34653 ,0.34709 ,0.34765 ,0.34820 ,0.34876 ,0.34931 ,0.34986 ,0.35041 ,0.35095 ,0.35150 ,0.35205 ,0.35259 ,0.35313 ,0.35367 ,0.35421 ,0.35475 ,0.35528 ,0.35582 ,0.35635 ,0.35688 ,0.35741 ,0.35794 ,0.35847 ,0.35899 ,0.35952 ,0.36004 ,0.36056 ,0.36108 ,0.36160 ,0.36212 ,0.36264 ,0.36315 ,0.36366 ,0.36418 ,0.36469 ,0.36520 ,0.36570 ,0.36621 ,0.36672 ,0.36722 ,0.36772 ,0.36823 ,0.36873 ,0.36923 ,0.36972 ,0.37022 ,0.37071 ,0.37121 ,0.37170 ,0.37219 ,0.37268 ,0.37317 ,0.37366 ,0.37415 ,0.37463 ,0.37512 ,0.37560 ,0.37608 ,0.37656 ,0.37704 ,0.37752 ,0.37799 ,0.37847 ,0.37894 ,0.37942 ,0.37989 ,0.38036 ,0.38083 ,0.38130 ,0.38177 ,0.38223 ,0.38270 ,0.38316 ,0.38362 ,0.38409 ,0.38455 ,0.38500 ,0.38546 ,0.38592 ,0.38638 ,0.38683 ,0.38728 ,0.38774 ,0.38819 ,0.38864 ,0.38909 ,0.38954 ,0.38998 ,0.39043 ,0.39087 ,0.39132 ,0.39176 ,0.39220 ,0.39264 ,0.39308 ,0.39352 ,0.39396 ,0.39439 ,0.39483 ,0.39526 ,0.39569 ,0.39612 ,0.39656 ,0.39699 ,0.39741 ,0.39784 ,0.39827 ,0.39869 ,0.39912 ,0.39954 ,0.39996 ,0.40038 ,0.40081 ,0.40122 ,0.40164 ,0.40206 ,0.40248 ,0.40289 ,0.40331 ,0.40372 ,0.40413 ,0.40454 ,0.40495 ,0.40536 ,0.40577 ,0.40618 ,0.40658 ,0.40699 ,0.40739 ,0.40779 ,0.40820 ,0.40860 ,0.40900 ,0.40940 ,0.40979 ,0.41019 ,0.41059 ,0.41098 ,0.41138 ,0.41177 ,0.41216 ,0.41255 ,0.41294 ,0.41333 ,0.41372 ,0.41411 ,0.41449 ,0.41488 ,0.41526 ,0.41565 ,0.41603 ,0.41641 ,0.41679 ,0.41717 ,0.41755 ,0.41793 ,0.41831 ,0.41868 ,0.41906 ,0.41943 ,0.41980 ,0.42018 ,0.42055 ,0.42092 ,0.42129 ,0.42166 ,0.42202 ,0.42239 ,0.42276 ,0.42312 ,0.42348 ,0.42385 ,0.42421 ,0.42457 ,0.42493 ,0.42529 ,0.42565 ,0.42601 ,0.42636 ,0.42672 ,0.42707 ,0.42743 ,0.42778 ,0.42813 ,0.42848 ,0.42883 ,0.42918 ,0.42953 ,0.42988 ,0.43023 ,0.43057 ,0.43092 ,0.43126 ,0.43160 ,0.43195 ,0.43229 ,0.43263 ,0.43297 ,0.43331 ,0.43364 ,0.43398 ,0.43432 ,0.43465 ,0.43499 ,0.43532 ,0.43565 ,0.43599 ,0.43632 ,0.43665 ,0.43698 ,0.43730 ,0.43763 ,0.43796 ,0.43828 ,0.43861 ,0.43893 ,0.43926 ,0.43958 ,0.43990 ,0.44022 ,0.44054 ,0.44086 ,0.44118 ,0.44150 ,0.44182 ,0.44213 ,0.44245 ,0.44276 ,0.44307 ,0.44339 ,0.44370 ,0.44401 ,0.44432 ,0.44463 ,0.44494 ,0.44525 ,0.44555 ,0.44586 ,0.44617 ,0.44647 ,0.44677 ,0.44708 ,0.44738 ,0.44768 ,0.44798 ,0.44828 ,0.44858 ,0.44888 ,0.44918 ,0.44947 ,0.44977 ,0.45007 ,0.45036 ,0.45065 ,0.45095 ,0.45124 ,0.45153 ,0.45182 ,0.45211 ,0.45240 ,0.45269 ,0.45298 ,0.45327 ,0.45355 ,0.45384 ,0.45412 ,0.45441 ,0.45469 ,0.45497 ,0.45526 ,0.45554 ,0.45582 ,0.45610 ,0.45638 ,0.45665 ,0.45693 ,0.45721 ,0.45749 ,0.45776 ,0.45804 ,0.45831 ,0.45858 ,0.45886 ,0.45913 ,0.45940 ,0.45967 ,0.45994 ,0.46021 ,0.46048 ,0.46075 ,0.46102 ,0.46128 ,0.46155 ,0.46181 ,0.46208 ,0.46234 ,0.46261 ,0.46287 ,0.46313 ,0.46339 ,0.46366 ,0.46392 ,0.46418 ,0.46443 ,0.46469 ,0.46495 ,0.46521 ,0.46546 ,0.46572 ,0.46598 ,0.46623 ,0.46649 ,0.46674 ,0.46699 ,0.46724 ,0.46750 ,0.46775 ,0.46800 ,0.46825 ,0.46850 ,0.46875 ,0.46899 ,0.46924 ,0.46949 ,0.46974 ,0.46998 ,0.47023 ,0.47047 ,0.47072 ,0.47096 ,0.47120 ,0.47145 ,0.47169 ,0.47193 ,0.47217 ,0.47241 ,0.47265 ,0.47289 ,0.47313 ,0.47337 ,0.47361 ,0.47385 ,0.47408 ,0.47432 ,0.47456 ,0.47479 ,0.47503 ,0.47526 ,0.47550 ,0.47573 ,0.47596 ,0.47620 ,0.47643 ,0.47666 ,0.47689 ,0.47712 ,0.47735 ,0.47758 ,0.47781 ,0.47804 ,0.47827 ,0.47850 ,0.47873 ,0.47895 ,0.47918 ,0.47941 ,0.47963 ,0.47986 ,0.48008 ,0.48031 ,0.48053 ,0.48076 ,0.48098 ,0.48120 ,0.48143 ,0.48165 ,0.48187 ,0.48209 ,0.48232 ,0.48254 ,0.48276 ,0.48298 ,0.48320 ,0.48342 ,0.48364 ,0.48385 ,0.48407 ,0.48429 ,0.48451 ,0.48473 ,0.48494 ,0.48516 ,0.48538 ,0.48559 ,0.48581 ,0.48603 ,0.48624 ,0.48646 ,0.48667 ,0.48688 ,0.48710 ,0.48731 ,0.48753 ,0.48774 ,0.48795 ,0.48816 ,0.48838 ,0.48859 ,0.48880 ,0.48901 ,0.48922 ,0.48943 ,0.48965 ,0.48986 ,0.49007 ,0.49028 ,0.49049 ,0.49070 ,0.49091 ,0.49111 ,0.49132 ,0.49153 ,0.49174 ,0.49195 ,0.49216 ,0.49236 ,0.49257 ,0.49278 ,0.49299 ,0.49319 ,0.49340 ,0.49361 ,0.49381 ,0.49402 ,0.49423 ,0.49443 ,0.49464 ,0.49484 ,0.49505 ,0.49525 ,0.49546 ,0.49566 ,0.49587 ,0.49607 ,0.49628 ,0.49648 ,0.49669 ,0.49689 ,0.49709 ,0.49730 ,0.49750 ,0.49770 ,0.49791 ,0.49811 ,0.49831 ,0.49852 ,0.49872 ,0.49892 ,0.49912 ,0.49933 ,0.49953 ,0.49973 ,0.49993 ,0.50014 ,0.50034 ,0.50054 ,0.50074 ,0.50094 ,0.50115 ,0.50135 ,0.50155 ,0.50175 ,0.50195 ,0.50215 ,0.50235 ,0.50255 ,0.50276 ,0.50296 ,0.50316 ,0.50336 ,0.50356 ,0.50376 ,0.50396 ,0.50416 ,0.50436 ,0.50456 ,0.50476 ,0.50496 ,0.50516 ,0.50536 ,0.50556 ,0.50576 ,0.50596 ,0.50616 ,0.50636 ,0.50656 ,0.50676 ,0.50696 ,0.50716 ,0.50736 ,0.50756 ,0.50776 ,0.50796 ,0.50816 ,0.50836 ,0.50856 ,0.50876 ,0.50896 ,0.50916 ,0.50936 ,0.50956 ,0.50976 ,0.50996 ,0.51016 ,0.51036 ,0.51055 ,0.51075 ,0.51095 ,0.51115 ,0.51135 ,0.51155 ,0.51175 ,0.51195 ,0.51215 ,0.51235 ,0.51255 ,0.51274 ,0.51294 ,0.51314 ,0.51334 ,0.51354 ,0.51374 ,0.51394 ,0.51414 ,0.51434 ,0.51453 ,0.51473 ,0.51493 ,0.51513 ,0.51533 ,0.51553 ,0.51572 ,0.51592 ,0.51612 ,0.51632 ,0.51652 ,0.51672 ,0.51691 ,0.51711 ,0.51731 ,0.51751 ,0.51770 ,0.51790 ,0.51810 ,0.51830 ,0.51849 ,0.51869 ,0.51889 ,0.51909 ,0.51928 ,0.51948 ,0.51968 ,0.51987 ,0.52007 ,0.52027 ,0.52046 ,0.52066 ,0.52085 ,0.52105 ,0.52125 ,0.52144 ,0.52164 ,0.52183 ,0.52203 ,0.52222 ,0.52242 ,0.52261 ,0.52281 ,0.52300 ,0.52320 ,0.52339 ,0.52358 ,0.52378 ,0.52397 ,0.52416 ,0.52436 ,0.52455 ,0.52474 ,0.52493 ,0.52513 ,0.52532 ,0.52551 ,0.52570 ,0.52589 ,0.52608 ,0.52627 ,0.52646 ,0.52665 ,0.52684 ,0.52703 ,0.52722 ,0.52741 ,0.52760 ,0.52779 ,0.52797 ,0.52816 ,0.52835 ,0.52853 ,0.52872 ,0.52890 ,0.52909 ,0.52927 ,0.52946 ,0.52964 ,0.52983 ,0.53001 ,0.53019 ,0.53037 ,0.53056 ,0.53074 ,0.53092 ,0.53110 ,0.53128 ,0.53146 ,0.53164 ,0.53181 ,0.53199 ,0.53217 ,0.53234 ,0.53252 ,0.53269 ,0.53287 ,0.53304 ,0.53322 ,0.53339 ,0.53356 ,0.53373 ,0.53390 ,0.53407 ,0.53424 ,0.53441 ,0.53458 ,0.53474 ,0.53491 ,0.53507 ,0.53524 ,0.53540 ,0.53556 ,0.53573 ,0.53589 ,0.53605 ,0.53621 ,0.53636 ,0.53652 ,0.53668 ,0.53683 ,0.53699 ,0.53714 ,0.53729 ,0.53744 ,0.53759 ,0.53774 ,0.53789 ,0.53804 ,0.53818 ,0.53833 ,0.53847 ,0.53862 ,0.53876 ,0.53890 ,0.53904 ,0.53917 ,0.53931 ,0.53945 ,0.53958 ,0.53971 ,0.53984 ,0.53997 ,0.54010 ,0.54023 ,0.54035 ,0.54048 ,0.54060 ,0.54072 ,0.54084 ,0.54096 ,0.54108 ,0.54119 ,0.54130 ,0.54142 ,0.54153 ,0.54163 ,0.54174 ,0.54185 ,0.54195 ,0.54205 ,0.54215 ,0.54225 ,0.54235 ,0.54244 ,0.54253 ,0.54262 ,0.54271 ,0.54280 ,0.54288 ,0.54297 ,0.54305 ,0.54313 ,0.54320 ,0.54328 ,0.54335 ,0.54342 ,0.54349 ,0.54355 ,0.54362 };
static float focusTable[] = {50,50.1,50.2,50.3,50.4,50.5,50.6,50.7,50.8,50.9,51,51.1,51.2,51.3,51.4,51.5,51.6,51.7,51.8,51.9,52,52.1,52.2,52.3,52.4,52.5,52.6,52.7,52.8,52.9,53,53.1,53.2,53.3,53.4,53.5,53.6,53.7,53.8,53.9,54,54.1,54.2,54.3,54.4,54.5,54.6,54.7,54.8,54.9,55,55.1,55.2,55.3,55.4,55.5,55.6,55.7,55.8,55.9,56,56.1,56.2,56.3,56.4,56.5,56.6,56.7,56.8,56.9,57,57.1,57.2,57.3,57.4,57.5,57.6,57.7,57.8,57.9,58,58.1,58.2,58.3,58.4,58.5,58.6,58.7,58.8,58.9,59,59.1,59.2,59.3,59.4,59.5,59.6,59.7,59.8,59.9,60,60.1,60.2,60.3,60.4,60.5,60.6,60.7,60.8,60.9,61,61.1,61.2,61.3,61.4,61.5,61.6,61.7,61.8,61.9,62,62.1,62.2,62.3,62.4,62.5,62.6,62.7,62.8,62.9,63,63.1,63.2,63.3,63.4,63.5,63.6,63.7,63.8,63.9,64,64.1,64.2,64.3,64.4,64.5,64.6,64.7,64.8,64.9,65,65.1,65.2,65.3,65.4,65.5,65.6,65.7,65.8,65.9,66,66.1,66.2,66.3,66.4,66.5,66.6,66.7,66.8,66.9,67,67.1,67.2,67.3,67.4,67.5,67.6,67.7,67.8,67.9,68,68.1,68.2,68.3,68.4,68.5,68.6,68.7,68.8,68.9,69,69.1,69.2,69.3,69.4,69.5,69.6,69.7,69.8,69.9,70,70.1,70.2,70.3,70.4,70.5,70.6,70.7,70.8,70.9,71,71.1,71.2,71.3,71.4,71.5,71.6,71.7,71.8,71.9,72,72.1,72.2,72.3,72.4,72.5,72.6,72.7,72.8,72.9,73,73.1,73.2,73.3,73.4,73.5,73.6,73.7,73.8,73.9,74,74.1,74.2,74.3,74.4,74.5,74.6,74.7,74.8,74.9,75,75.1,75.2,75.3,75.4,75.5,75.6,75.7,75.8,75.9,76,76.1,76.2,76.3,76.4,76.5,76.6,76.7,76.8,76.9,77,77.1,77.2,77.3,77.4,77.5,77.6,77.7,77.8,77.9,78,78.1,78.2,78.3,78.4,78.5,78.6,78.7,78.8,78.9,79,79.1,79.2,79.3,79.4,79.5,79.6,79.7,79.8,79.9,80,80.1,80.2,80.3,80.4,80.5,80.6,80.7,80.8,80.9,81,81.1,81.2,81.3,81.4,81.5,81.6,81.7,81.8,81.9,82,82.1,82.2,82.3,82.4,82.5,82.6,82.7,82.8,82.9,83,83.1,83.2,83.3,83.4,83.5,83.6,83.7,83.8,83.9,84,84.1,84.2,84.3,84.4,84.5,84.6,84.7,84.8,84.9,85,85.1,85.2,85.3,85.4,85.5,85.6,85.7,85.8,85.9,86,86.1,86.2,86.3,86.4,86.5,86.6,86.7,86.8,86.9,87,87.1,87.2,87.3,87.4,87.5,87.6,87.7,87.8,87.9,88,88.1,88.2,88.3,88.4,88.5,88.6,88.7,88.8,88.9,89,89.1,89.2,89.3,89.4,89.5,89.6,89.7,89.8,89.9,90,90.1,90.2,90.3,90.4,90.5,90.6,90.7,90.8,90.9,91,91.1,91.2,91.3,91.4,91.5,91.6,91.7,91.8,91.9,92,92.1,92.2,92.3,92.4,92.5,92.6,92.7,92.8,92.9,93,93.1,93.2,93.3,93.4,93.5,93.6,93.7,93.8,93.9,94,94.1,94.2,94.3,94.4,94.5,94.6,94.7,94.8,94.9,95,95.1,95.2,95.3,95.4,95.5,95.6,95.7,95.8,95.9,96,96.1,96.2,96.3,96.4,96.5,96.6,96.7,96.8,96.9,97,97.1,97.2,97.3,97.4,97.5,97.6,97.7,97.8,97.9,98,98.1,98.2,98.3,98.4,98.5,98.6,98.7,98.8,98.9,99,99.1,99.2,99.3,99.4,99.5,99.6,99.7,99.8,99.9,100,100.1,100.2,100.3,100.4,100.5,100.6,100.7,100.8,100.9,101,101.1,101.2,101.3,101.4,101.5,101.6,101.7,101.8,101.9,102,102.1,102.2,102.3,102.4,102.5,102.6,102.7,102.8,102.9,103,103.1,103.2,103.3,103.4,103.5,103.6,103.7,103.8,103.9,104,104.1,104.2,104.3,104.4,104.5,104.6,104.7,104.8,104.9,105,105.1,105.2,105.3,105.4,105.5,105.6,105.7,105.8,105.9,106,106.1,106.2,106.3,106.4,106.5,106.6,106.7,106.8,106.9,107,107.1,107.2,107.3,107.4,107.5,107.6,107.7,107.8,107.9,108,108.1,108.2,108.3,108.4,108.5,108.6,108.7,108.8,108.9,109,109.1,109.2,109.3,109.4,109.5,109.6,109.7,109.8,109.9,110,110.1,110.2,110.3,110.4,110.5,110.6,110.7,110.8,110.9,111,111.1,111.2,111.3,111.4,111.5,111.6,111.7,111.8,111.9,112,112.1,112.2,112.3,112.4,112.5,112.6,112.7,112.8,112.9,113,113.1,113.2,113.3,113.4,113.5,113.6,113.7,113.8,113.9,114,114.1,114.2,114.3,114.4,114.5,114.6,114.7,114.8,114.9,115,115.1,115.2,115.3,115.4,115.5,115.6,115.7,115.8,115.9,116,116.1,116.2,116.3,116.4,116.5,116.6,116.7,116.8,116.9,117,117.1,117.2,117.3,117.4,117.5,117.6,117.7,117.8,117.9,118,118.1,118.2,118.3,118.4,118.5,118.6,118.7,118.8,118.9,119,119.1,119.2,119.3,119.4,119.5,119.6,119.7,119.8,119.9,120,120.1,120.2,120.3,120.4,120.5,120.6,120.7,120.8,120.9,121,121.1,121.2,121.3,121.4,121.5,121.6,121.7,121.8,121.9,122,122.1,122.2,122.3,122.4,122.5,122.6,122.7,122.8,122.9,123,123.1,123.2,123.3,123.4,123.5,123.6,123.7,123.8,123.9,124,124.1,124.2,124.3,124.4,124.5,124.6,124.7,124.8,124.9,125,125.1,125.2,125.3,125.4,125.5,125.6,125.7,125.8,125.9,126,126.1,126.2,126.3,126.4,126.5,126.6,126.7,126.8,126.9,127,127.1,127.2,127.3,127.4,127.5,127.6,127.7,127.8,127.9,128,128.1,128.2,128.3,128.4,128.5,128.6,128.7,128.8,128.9,129,129.1,129.2,129.3,129.4,129.5,129.6,129.7,129.8,129.9,130,130.1,130.2,130.3,130.4,130.5,130.6,130.7,130.8,130.9,131,131.1,131.2,131.3,131.4,131.5,131.6,131.7,131.8,131.9,132,132.1,132.2,132.3,132.4,132.5,132.6,132.7,132.8,132.9,133,133.1,133.2,133.3,133.4,133.5,133.6,133.7,133.8,133.9,134,134.1,134.2,134.3,134.4,134.5,134.6,134.7,134.8,134.9,135,135.1,135.2,135.3,135.4,135.5,135.6,135.7,135.8,135.9,136,136.1,136.2,136.3,136.4,136.5,136.6,136.7,136.8,136.9,137,137.1,137.2,137.3,137.4,137.5,137.6,137.7,137.8,137.9,138,138.1,138.2,138.3,138.4,138.5,138.6,138.7,138.8,138.9,139,139.1,139.2,139.3,139.4,139.5,139.6,139.7,139.8,139.9,140,140.1,140.2,140.3,140.4,140.5,140.6,140.7,140.8,140.9,141,141.1,141.2,141.3,141.4,141.5,141.6,141.7,141.8,141.9,142,142.1,142.2,142.3,142.4,142.5,142.6,142.7,142.8,142.9,143,143.1,143.2,143.3,143.4,143.5,143.6,143.7,143.8,143.9,144,144.1,144.2,144.3,144.4,144.5,144.6,144.7,144.8,144.9,145,145.1,145.2,145.3,145.4,145.5,145.6,145.7,145.8,145.9,146,146.1,146.2,146.3,146.4,146.5,146.6,146.7,146.8,146.9,147,147.1,147.2,147.3,147.4,147.5,147.6,147.7,147.8,147.9,148,148.1,148.2,148.3,148.4,148.5,148.6,148.7,148.8,148.9,149,149.1,149.2,149.3,149.4,149.5,149.6,149.7,149.8,149.9,150};

static bool motorShouldStop = true;
static int OVER_TIME = 10000;//电机运行超时时间，单位（ms）
static bool isSelfCheckStateOK = true;

float AdRatioToFocus(float ratio)
{
    long size = sizeof(ratioTable) / sizeof(float);
    int key = std::lower_bound(ratioTable, ratioTable + size, ratio) - ratioTable;
    float focus = 0;
    if(key>=size-1)
    {
        focus = focusTable[size-1];
        return focus;
    }
    if(key<=0)
    {
        focus = focusTable[0];
        return focus;
    }
    if (ratio!= ratioTable[key])
    {
        focus = (focusTable[key + 1] - focusTable[key]) / (ratioTable[key + 1] - ratioTable[key])*(ratio - ratioTable[key]) + focusTable[key];
    }
    else
    {
        focus = focusTable[key];
    }
    return CheckFocusRange(focus);
}

float GetFocusNow()
{
    return AdRatioToFocus(GetAdRatioNow());
}

float GetFocusAverage()
{
    return AdRatioToFocus(GetAdRatioAverage());
}

float GetFocusAverageWait(int timeMs)//电机停止运行一段时间，获取这段时间的平均值
{
//    MotorEnable(false);
    std::this_thread::sleep_for(std::chrono::milliseconds(timeMs));
//    MotorEnable(true);
    return GetFocusAverage();
}

//void MotorRun(int targetFocus)//修正超调版本
//{
//    motorShouldStop = true;
//    while(GetMotorRunState())
//    {
//        printf("motor has a command and is running, wait for last command finish\n");
//        std::this_thread::sleep_for(std::chrono::milliseconds(10));
//    }
//    printf("start run now, target focus is %d\n",targetFocus);
//    motorShouldStop = false;

//    MotorEnable(true);
//    float speed = 20;

//    auto startTime = std::chrono::system_clock::now();
//    double timeCost;
//    while(!motorShouldStop)
//    {
//        float focus = GetFocusNow();
//        printf("before motor cycle run, now the focus is %.1f\n",focus);
//        float diff = targetFocus-focus;
////        printf("before cal diff,now the focus is %.1f\n",focusNow);
////        printf("before cal diff,targetFocus is %.1f\n",targetFocus);
//        printf("diff is %.2f\n",diff);

//        if(focus<90)
//        {
//            if(fabs(diff)<2)
//            {
//                speed = speed-0.5;
//                if(speed<=20)
//                {
//                    speed = 20;
//                }
//            }
//            else
//            {
//                speed = speed+0.5;
//                if(speed>=50)
//                {
//                    speed = 50;
//                }
//            }
//        }
//        else
//        {
//            speed = speed+0.5;
//            if(speed>=50)
//            {
//                speed = 50;
//            }
//        }

//        if(diff>0.1)
//        {
//            if(!GetMotorDirIsAdd())
//            {
//                MotorDirFocusAdd();
//                printf("reset speed\n");
//                speed = 20;
//            }

//        }
//        else if(diff<-0.1)
//        {
//            if(GetMotorDirIsAdd())
//            {
//                MotorDirFocusDec();
//                printf("reset speed\n");
//                speed = 20;
//            }

//        }
//        else
//        {
//            printf("diff is small, so stop Motor\n");
//            MotorEnable(false);
//            break;
//        }
//        MotorSetSpeed(speed);
//        std::this_thread::sleep_for(std::chrono::milliseconds(5));
//        auto endTime = std::chrono::system_clock::now();
//        auto duration = std::chrono::duration_cast<std::chrono::seconds>(endTime - startTime);
//        timeCost = static_cast<double>(duration.count());
//        printf("time cost %.2f\n",timeCost);
//        if(timeCost>OVER_TIME)
//        {
//            printf("selfCheck error\n");
//            isSelfCheckStateOK = false;
//            break;
//        }
//        printf("after motor cycle run, now the focus is %.1f\n",GetFocusNow());
//    }

//    if(timeCost<=OVER_TIME)
//    {
//        printf("selfCheck OK\n");
//        isSelfCheckStateOK = true;
//    }

//    MotorEnable(false);
//}


void MotorRun(int targetFocus)//不修正超调版本
{
    if(!CamControl::GetInstance().GetWorkMode())
    {
        MotorEnable(false);
        return;
    }

    motorShouldStop = true;
    while(GetMotorRunState())
    {
        printf("motor has a command and is running, wait for last command finish\n");
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
//    MotorEnable(false);
    float focusAverage = GetFocusAverageWait(20);
    float diffBeforeRun = static_cast<float>(targetFocus)-focusAverage;
//    printf("before motor run, average focus is %.2f\n",focusAverage);
//    printf("before motor run, target focus is %d\n",targetFocus);
//    printf("before motor run, diff is %.2f\n",diffBeforeRun);

    float stopDiffBeforRun = 0.1;
    if(diffBeforeRun>stopDiffBeforRun)
    {
         MotorDirFocusAdd();
         printf("focus should add\n");
    }
    else if(diffBeforeRun<(-stopDiffBeforRun))
    {
        MotorDirFocusDec();
        printf("focus should dec\n");
    }
    else
    {
        printf("diff is too small, motor should not run\n");
//        MotorEnable(false);
//        motorShouldStop = false;
        return;
    }

    motorShouldStop = false;
    MotorEnable(true);
    float speed = 20;

    auto startTime = std::chrono::system_clock::now();
    double timeCost = 0;
    float focusNow;
    float diffNow;
    int suitCount = 0;
    int lagerCount = 0;
    int smallerCount = 0;
    while(!motorShouldStop)
    {
        if(!CamControl::GetInstance().GetWorkMode())
        {
            MotorEnable(false);
            return;
        }

        focusNow = GetFocusNow();
        diffNow = static_cast<float>(targetFocus)-focusNow;
//        printf("motor running, now focus is %.2f\n",focusNow);
//        printf("motor running, target focus is %d\n",targetFocus);
//        printf("motor running, diff is %.2f\n",diffNow);

        float slowDownDiff;//目标焦距和现在焦距差距多少时开始减速
        float stopDiffRunning;//目标焦距和现在焦距差距多少时电机停止
        int cycleTime;//一个周期的运行时间（ms）

        if(focusNow<60)
        {
            slowDownDiff = 0.5;
            stopDiffRunning = 0.3;
            cycleTime = 6;
        }
        else if(focusNow<70)
        {
            slowDownDiff = 1;
            stopDiffRunning = 0.2;
            cycleTime = 5;
        }
        else if(focusNow<110)
        {
            slowDownDiff = 3;
            stopDiffRunning = 0.1;
            cycleTime = 4;
        }
        else
        {
            slowDownDiff = 6;
            stopDiffRunning = 0.01;
            cycleTime = 3;
        }

        if(fabs(diffNow)<slowDownDiff)
        {
            speed = speed-0.5;
            if(speed<=20)
            {
                speed = 20;
            }
        }
        else
        {
            speed = speed+0.5;
            if(speed>=50)
            {
                speed = 50;
            }
        }

        if(fabs(diffNow)<stopDiffRunning)
        {
            suitCount++;
            lagerCount = 0;
            smallerCount = 0;
//            printf("suitCount = %d\n",suitCount);
            if(suitCount>=3)
            {
                printf("diff is small, so stop Motor\n");
                break;
            }

        }
        else if(diffNow>0)
        {
            smallerCount = 0;
            suitCount = 0;
            if(!GetMotorDirIsAdd())
            {
                lagerCount++;
//                printf("lagerCount = %d\n",lagerCount);
                if(lagerCount>=2)
                {
//                    printf("focus is dec, target now is larger than focus,so stop\n");
                    break;
                }
            }
        }
        else if(diffNow<0)
        {
            lagerCount = 0;
            suitCount = 0;
            if(GetMotorDirIsAdd())
            {
                smallerCount++;
//                printf("smallerCount = %d\n",smallerCount);
                if(smallerCount>=2)
                {
//                    printf("focus is add, target now is smaller than focus,so stop\n");
                    break;
                }
            }
        }
        MotorSetSpeed(speed);
        std::this_thread::sleep_for(std::chrono::milliseconds(cycleTime));
        auto endTime = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
        timeCost = static_cast<double>(duration.count());
//        printf("time cost %.1fms\n",timeCost);
        if(timeCost>OVER_TIME)
        {
            printf("selfCheck error\n");
            isSelfCheckStateOK = false;
            break;
        }
//        printf("after motor run a cycle, now the focus is %.1f\n",GetFocusNow());
    }
    if(timeCost<=OVER_TIME)
    {
        printf("selfCheck OK\n");
        isSelfCheckStateOK = true;
    }
    MotorEnable(false);
}

void MotorAddFocus(int step)
{
    int targetFocusInt = CheckFocusRange(GetFocusNow()+step+0.5);
    MotorRun(targetFocusInt);
}

void MotorDecFocus(int step)
{
    int targetFocusInt = CheckFocusRange(GetFocusNow()-step+0.5);
    MotorRun(targetFocusInt);
}

void MotorFocusToTarget(int value)
{
    int targetFocusInt = CheckFocusRange(value);
    MotorRun(targetFocusInt);
}

#endif//#ifndef MCU_IS_ALIVE

FocusControl focusCtl;

FocusControl::FocusControl()
{
#ifdef MCU_COMM_TYPE_IS_QT
    connect(this,&FocusControl::IncFocusSignal,&ProtocolMCU::GetInstance(),static_cast<void(ProtocolMCU::*)(unsigned char)>(&ProtocolMCU::IncFocus));
    connect(this,&FocusControl::DecFocusSignal,&ProtocolMCU::GetInstance(),static_cast<void(ProtocolMCU::*)(unsigned char)>(&ProtocolMCU::DecFocus));
    connect(this,&FocusControl::SetFocusSignal,&ProtocolMCU::GetInstance(),static_cast<void(ProtocolMCU::*)(unsigned char)>(&ProtocolMCU::SetFocus));
    connect(this,&FocusControl::GetFocusCommandSignal,&ProtocolMCU::GetInstance(),&ProtocolMCU::GetFocusCommand);
    connect(this,&FocusControl::SetWorkModeSignal,&ProtocolMCU::GetInstance(),&ProtocolMCU::SetWorkMode);
    connect(this,&FocusControl::MotoSelfCheckSignal,&ProtocolMCU::GetInstance(),static_cast<void(ProtocolMCU::*)()>(&ProtocolMCU::MotoSelfCheck));
#endif//#ifdef MCU_COMM_TYPE_IS_QT
}

void FocusControl::Init()
{
#ifndef MCU_IS_ALIVE
    MotorInit();
    std::thread(AdSpiRead).detach();
 #endif //#ifndef MCU_IS_ALIVE
}

void FocusControl::IncFocus(unsigned char step)
{
#ifdef MCU_IS_ALIVE
#ifdef MCU_COMM_TYPE_IS_QT
    emit IncFocusSignal(step);
#else
    ProtocolMCU::GetInstance().IncFocus(step);
#endif//#ifdef MCU_COMM_TYPE_IS_QT
#else
    std::thread(MotorAddFocus, step).detach();
#endif //#ifdef MCU_IS_ALIVE
}

void FocusControl::DecFocus(unsigned char step)
{
#ifdef MCU_IS_ALIVE
#ifdef MCU_COMM_TYPE_IS_QT
    emit DecFocusSignal(step);
#else
    ProtocolMCU::GetInstance().DecFocus(step);
#endif//#ifdef MCU_COMM_TYPE_IS_QT
#else
    std::thread(MotorDecFocus,step).detach();
#endif //#ifdef MCU_IS_ALIVE
}

void FocusControl::SetFocus(unsigned char value)
{
    int targetFocusInt = CheckFocusRange(value);
#ifdef MCU_IS_ALIVE
#ifdef MCU_COMM_TYPE_IS_QT
    emit SetFocusSignal(targetFocusInt);
#else
    ProtocolMCU::GetInstance().SetFocus(targetFocusInt);
#endif//#ifdef MCU_COMM_TYPE_IS_QT
#else
    std::thread(MotorFocusToTarget,targetFocusInt).detach();
#endif //#ifdef MCU_IS_ALIVE
}

void FocusControl::SetView(float value)
{

}

//void FocusControl::GetFocusCommand()
//{
//#ifdef MCU_COMM_TYPE_IS_QT
//    emit GetFocusCommandSignal();
//#else
//    ProtocolMCU::GetInstance().GetFocusCommand();
//#endif//#ifdef MCU_COMM_TYPE_IS_QT
//}

void FocusControl::SetWorkMode(bool mode)
{
#ifdef MCU_IS_ALIVE
#ifdef MCU_COMM_TYPE_IS_QT
    emit SetWorkModeSignal(mode);
#else
    ProtocolMCU::GetInstance().SetWorkMode(mode);
#endif//#ifdef MCU_COMM_TYPE_IS_QT
#else

#endif //#ifdef MCU_IS_ALIVE
}

void FocusControl::MotoSelfCheck()
{
#ifdef MCU_IS_ALIVE
#ifdef MCU_COMM_TYPE_IS_QT
    emit MotoSelfCheckSignal();
#else
    ProtocolMCU::GetInstance().MotoSelfCheck();
#endif//#ifdef MCU_COMM_TYPE_IS_QT
#else
    std::thread([&](){
        MotorFocusToTarget(FOCUS_MIN);
        MotorFocusToTarget(FOCUS_MAX);
    }).detach();

#endif //#ifdef MCU_IS_ALIVE
}

unsigned char FocusControl::GetFocus()
{
#ifdef MCU_IS_ALIVE
    return ProtocolMCU::GetInstance().GetFocus();
#else
    float focus;
    if(GetMotorRunState())
    {
        focus = GetFocusNow();
    }
    else
    {
        focus = GetFocusAverage();
    }
    return CheckFocusRange(focus+0.5);
#endif//#ifdef MCU_IS_ALIVE
}

bool FocusControl::GetSelfCheckState()
{
#ifdef MCU_IS_ALIVE
    return ProtocolMCU::GetInstance().GetSelfCheckState();
#else
    return isSelfCheckStateOK;
#endif//#ifdef MCU_IS_ALIVE
}

float FocusControl::GetViewField()
{
    unsigned char focusNow = GetFocus();
    if(focusNow>0)
    {
        return 2*atan(ICCD_CATHODE_RADIUS/focusNow)*RADIAN;
    }
    else
    {
        return 6.9;
    }
}

/********************************************************************\
 *
 * REVISION RECORDS
 *
\*********************************************************************/
/*********************************************************************/
/*
 *
 *
 *
\*********************************************************************/
/*------------------------------The End------------------------------*/
